
#include "packer.h"

namespace Packing
{
	Packer* Packer::instance_ = 0 ;
	
	Packer::Packer(const std::string& poly_fn, double coverage_rate)
	{
		gx_assert(instance_ == 0) ;
		instance_ = this ;
		std::srand(std::time(0));

		use_voronoi = true;
		allow_enlarge = false;
		allow_swap = true;
		boost::random::mt19937 rand_gen(std::time(0));
		std::string ext = Geex::FileSystem::extension(poly_fn);
		read_polygon_set(poly_fn, std::back_inserter(polygon_set), ext);
		max_rate = coverage_rate;
		std::cout << "Specified coverage rate: "<<max_rate<<std::endl;
		// distribute the input polygons
		// Policy 1: random distribution
		// calculate a reasonable bounding box size
		double max_width = -std::numeric_limits<double>::max(), max_height = -std::numeric_limits<double>::max();
		total_volume = 0.0;
		for (unsigned int i = 0; i < polygon_set.size(); i++)
		{
			Packing_polygon::Iso_rectangle_2 bdbox = polygon_set[i]->bbox();
			max_width = std::max(bdbox.xmax() - bdbox.xmin(), max_width);
			max_height = std::max(bdbox.ymax() - bdbox.ymin(), max_height);
			if (polygon_set[i]->is_clockwise_oriented())
				polygon_set[i]->reverse_orientation();

			double vol = polygon_set[i]->volume();
			total_volume += vol;

		}
		max_width *= 3.5;
		cbox = new Rectangular_box(0.0, 0.0, max_width, total_volume/max_rate/max_width);

		///////////////////////	Initialization for geometric method	//////////////////////////
		std::vector<Rectangular_box::Point> bd_verts;
		dynamic_cast<Rectangular_box*>(cbox)->vertices(bd_verts);
		pt = new Polygon_triangulation(bd_verts, 200);
		std::for_each(polygon_set.begin(), polygon_set.end(), std::bind2nd(std::mem_fun(&Packing_polygon::relative_scale), 0.05));

		// grid sampling		
		unsigned int grid_dim_x = (cbox->x_max() - cbox->x_min())/(0.1*max_width) + 1;
		unsigned int grid_dim_y = (cbox->y_max() - cbox->y_min())/(0.1*max_height) + 1;
		double dx = (cbox->x_max() - cbox->x_min())/grid_dim_x, dy = (cbox->y_max() - cbox->y_min())/grid_dim_y;
		std::vector<unsigned int> pos;
		pos.reserve(grid_dim_x*grid_dim_y);
		for (unsigned int i = 0; i < grid_dim_x*grid_dim_y; i++)	
			//if (i/grid_dim_x == 0 || i%grid_dim == 0 || i/grid_dim == grid_dim - 1 || i%grid_dim == grid_dim - 1) // excluding boundary grids
			//	continue;
			//else
				pos.push_back(i);
		std::random_shuffle(pos.begin(), pos.end());
		for (unsigned int i = 0; i < polygon_set.size(); i++)
		{
			double lx = cbox->x_min() + pos[i]%grid_dim_x*dx, ly = cbox->y_min() + pos[i]/grid_dim_x*dy;
			double ux = lx + dx, uy = ly + dy;
			Packing_polygon::Vector v((lx+ux)/2,(ly+uy)/2), t;
			Packing_polygon::Point c = polygon_set[i]->centroid();
			t = v - (c - CGAL::ORIGIN);
			polygon_set[i]->translate(t.x(), t.y());
		}
		pt->insert_polygons(polygon_set);

		optimizer = new Optimizer(polygon_set, *pt);
		swapper = new Swapper(polygon_set, *pt);
		//////////////////////////////////////////////////////////////////////////////////////////////////

		///////////////////////	Initialization for energy method	//////////////////////////////////////
		//max_height = -std::numeric_limits<double>::max();
		//Bin_packing_2 bp(*dynamic_cast<Rectangular_box*>(cbox));
		//cus.resize(polygon_set.size());
		//for (unsigned int i = 0; i < polygon_set.size(); i++)
		//{
		//	double x, y;
		//	if (bp.put_one_bin(polygon_set[i]->bbox(), x, y))
		//		polygon_set[i]->translate(x, y);
		//	max_height = std::max(polygon_set[i]->bbox().ymax(), max_height);
		//	cus[i] = polygon_set[i]->medial_circles();
		//}
		//cbox->lower(cbox->y_max() - max_height);
		//std::ofstream ofs("dighe1_circle_unions.txt");
		//ofs << cus.size() << std::endl;
		//for (unsigned int i = 0; i < cus.size(); i++)
		//{
		//	ofs << cus[i].size() << std::endl;
		//	for (unsigned int j = 0; j < cus[i].size(); j++)
		//	{
		//		ofs<<cus[i].circle_at(j).center().x()<<' '<<cus[i].circle_at(j).center().y()<<' '<<cus[i].circle_at(j).squared_radius()<<std::endl;
		//	}
		//}
		//////////////////////////////////////////////////////////////////////////////////////////////////
		std::cout<<nb_polytopes()<<" polygons are to be packed...\n";
	}
	Packer::~Packer() 
	{
		delete cbox;
		delete optimizer;
		delete swapper;
		for (unsigned int i = 0; i < nb_polytopes(); i++)
			delete polygon_set[i];
		instance_ = 0 ; 
	}
	void Packer::update_dt()
	{
		bool insert_succeeds = false;
		while (!insert_succeeds)
		{
			try 
			{
				pt->clear();
				pt->insert_polygons(polygon_set);
				insert_succeeds = true;
			}
			catch (Intersect_constraints &e)
			{
				//std::cout<<">>>>>>> Intersection of constraints happened!!! Shrink the polygon "<<e.index()<<". <<<<<<<<\n";
				polygon_set[e.index()]->relative_scale(0.99);
			}
		}
	}

	void Packer::lloyd(unsigned int nb_iter, void (*post_action)())
	{
		CGAL::Timer lloyd_timer, cdt_timer;
		static double lloyd_time = 0.0;
		static double cdt_time = 0.0;
		for (unsigned int i = 0; i < nb_iter; i++)
		{
			lloyd_timer.start();
			optimizer->run_one_lloyd(allow_enlarge);
			lloyd_timer.stop();
			cdt_timer.start();
			update_dt();
			cdt_timer.stop();
			lloyd_time += lloyd_timer.time();
			cdt_time += cdt_timer.time();
		}
		if (post_action != NULL)
			post_action();
		//std::cout<<">>> Lloyd time = "<<lloyd_time<<", CDT time = "<<cdt_time<<" <<<\n";
	}

	void Packer::optimize()
	{
		System_potential_energy *spe = new Circle_union_system(cus, *dynamic_cast<Rectangular_box*>(cbox));
		double *trans = new double[cus.size()*3];
		std::fill(trans, trans + cus.size()*3, 0.0);

		//timer.start();

		int res = spe->optimize(trans, false);
		double e = std::numeric_limits<double>::max();	
	
		if (res > -200)
		{
			for (unsigned int i = 0; i < cus.size(); i++)
			{
				//std::cout<<'['<<trans[i*3]<<','<<trans[3*i+1]<<','<<trans[3*i+2]<<"], ";
				cus[i].transform(trans[i*3], trans[3*i+1], trans[3*i+2]);	
				polygon_set[i]->transform(Parameter(1.0, trans[i*3], trans[i*3+1], trans[i*3+2]));
			}

			//std::cout<<"Try to eliminate overlap...\n";
			//std::fill(trans, trans + cus.size()*3, 0.0);
			//res = spe->optimize(trans, true);
			//if (res > -200)
			//{
			//	// after query, we do update
			//	double ge = spe->gravity_energy_at(trans);
			//	e = spe->system_energy_at(trans);
			//	std::cout<<"Potential energy = "<<e<<", gravity energy = "<<ge<<", LJ energy = "<<e-ge<<std::endl;
			//	for (unsigned int i = 0; i < cus.size(); i++)
			//	{
			//		cus[i].transform(trans[i*3], trans[3*i+1], trans[3*i+2]);	
			//		polygon_set[i]->transform(Parameter(1.0, trans[i*3], trans[i*3+1], trans[i*3+2]));
			//	}
			//}
			//else
			//{
			//	std::cout<<"Failed to eliminate overlap, with error code = "<<res<<std::endl;
			//}
		}
		else
		{
			std::cout<<"Minimization failed, error code = "<<res<<std::endl;
		}

		//timer.stop();
		//opti_time += timer.time();
		//opti_nb++;
		//std::cout<<"Average optimization time = "<<opti_time/opti_nb<<std::endl;
		delete[] trans;
		delete spe;
	}
	double Packer::coverage_rate(double& max_factor, double& min_factor) const
	{
		// report
		double current_polygon_area = 0.0;
		double min_scale = 2.0, max_scale = -1.0;
		for (unsigned int i = 0; i < polygon_set.size(); i++)
		{
			current_polygon_area += polygon_set[i]->area();
			min_scale = std::min(min_scale, polygon_set[i]->get_scale_factor());
			max_scale = std::max(max_scale, polygon_set[i]->get_scale_factor());
		}
		double cov_rate = current_polygon_area/cbox->volume();
		//std::cout<<"Area coverage = "<<cov_rate<<',';
		//std::cout<<"min = "<<min_scale<<','<<"max = "<<max_scale<<std::endl;
		max_factor = max_scale;
		min_factor = min_scale;
		return cov_rate;
	}
	void Packer::pack(void (*post_action)())
	{
		unsigned int iter_times = 0, iter_limit = 1000;
		CGAL::Timer pack_timer;
		pack_timer.start();

		double maxf, minf;
		double pre_coverage, cur_coverage = coverage_rate(maxf, minf);
		allow_enlarge = false;
		// evenly distribute
		unsigned int n = 10;
		lloyd(n*3, post_action);
		
		while (cur_coverage < max_rate && iter_times < iter_limit)
		{
			pre_coverage = cur_coverage;
			allow_enlarge = true;
			lloyd(n, post_action);

			cur_coverage = coverage_rate(maxf, minf);
			if (std::fabs(cur_coverage - max_rate) <= 1.0e-5 || minf >= 1.0 )	break;
			allow_enlarge = false;
			if (cur_coverage - pre_coverage <= 1e-4)
			{
				shrink();
				//int d = rand();
				//if (allow_swap && d%iter_times == 0)
				//{
				//	swap();
				//	if(post_action)	post_action();
				//}
				//else
					lloyd(n*2, post_action);
				//iter_times++;
			}
			else
				lloyd(n, post_action);	
			std::cout<<"Area coverage = "<<cur_coverage<<',';
			std::cout<<"min = "<<minf<<", max = "<<maxf<<std::endl;
			iter_times++;
		}
		pack_timer.stop();

		std::cout<<"DONE! Time elapsed: "<<pack_timer.time()/60.0<<" minutes"<<std::endl;
	}
	void Packer::sa_pack(void (*post_action)())
	{
		CGAL::Timer sa_pack_timer;
		double minf, maxf;
		double pre_coverage, cur_coverage = coverage_rate(maxf, minf); // covearge rate
		double pre_energy = 0.0, cur_energy = cur_coverage; // NOTE: take energy function as the coverage rate. This should be reconsidered in the future.
		
		const double k = 1.0; // normalization factor
		const double cf = 0.9; // cooling factor
		double temp = 1.0; // temperature
		std::vector< Packing_polygon > pre_polytopes(nb_polytopes()); // previous polygon state

		sa_pack_timer.start();
		unsigned int n = 10; // Lloyd unit

		// relax at the beginning
		allow_enlarge = false;
		lloyd(n*3, post_action);

		double converge_threshold = 1.0e-5;
		size_t itn = 0;
		while ( !should_stop(cur_coverage, minf) )
		{
			std::cout<<"========== "<<itn++<<" ==========\n";
			cur_coverage = coverage_rate(maxf, minf);
			// go to a convergence state first
			do
			{
				pre_coverage = cur_coverage;
				allow_enlarge = true;
				lloyd(n, post_action);
				cur_coverage = coverage_rate(maxf, minf);
				if ( should_stop(cur_coverage, minf) )	break;
				allow_enlarge = false;
				if ( cur_coverage - pre_coverage <= converge_threshold*5.0 )
				{
					shrink();
					//if (post_action != NULL)	post_action();
				}
				lloyd(n, post_action);	
			} while ( cur_coverage - pre_coverage > converge_threshold );

			if ( should_stop(cur_coverage, minf) )	break;

			pre_energy = cur_coverage;

			// transit to another state
			// backup the current state first
			for (unsigned int i = 0; i < nb_polytopes(); i++)
				pre_polytopes[i] = *polygon_set[i];

			std::cout<<"In the previous state: \n";
			std::cout<<"Area coverage = "<<cur_coverage<<',';
			std::cout<<"min = "<<minf<<", max = "<<maxf<<std::endl;
			std::cout<<"Transitting to a new state...\n";
			if (Geex::Numeric::random_float64() < 0.5)
			{
				std::cout<<"Random shuffle...\n";
				swap();
			}
			else
			{
				std::cout<<"Fluctuate rotation...\n";
				swapper->fluctuate_rotation();
				update_dt();
			}
			if (post_action != NULL) post_action();

			cur_coverage = coverage_rate(maxf, minf);
			// go to a convergence state after transition
			do
			{
				pre_coverage = cur_coverage;
				allow_enlarge = true;
				lloyd(n, post_action);
				cur_coverage = coverage_rate(maxf, minf);
				if ( should_stop(cur_coverage, minf) )	break;
				allow_enlarge = false;
				if ( cur_coverage - pre_coverage <= converge_threshold*5.0 )
				{
					shrink();
					//if (post_action != NULL) post_action();
				}
				lloyd(n, post_action);	
			} while ( cur_coverage - pre_coverage > converge_threshold );

			std::cout<<"In the current state: \n";
			std::cout<<"Area coverage = "<<cur_coverage<<',';
			std::cout<<"min = "<<minf<<", max = "<<maxf<<std::endl;

			if ( should_stop(cur_coverage, minf) )	break;

			// check whether the new state is better or not
			cur_energy = cur_coverage;
			if (cur_energy > pre_energy)	// because enery is the coverage rate, the higher energy, the better
			{
				converge_threshold *= 2.0;
				std::cout<< "Accepted the new state, because of higher coverage.\n";
			}
			else
			{// whether to accept the worse case
				double p = std::pow(boost::math::constants::e<double>(), (cur_energy - pre_energy)/(k*temp));
				if ( Geex::Numeric::random_float64() <= p ) 
				{ // accept
					std::cout<<"Accepted the new state, because of probability("<<p<<").\n";
				}
				else
				{ // reject, go the previous state
					for (unsigned int i = 0; i < nb_polytopes(); i++)
						*polygon_set[i] = pre_polytopes[i];
					update_dt();
					if (post_action != NULL) post_action();
					// restrict the converge criterion
					converge_threshold = std::max(0.5*converge_threshold, 1.0e-6);
					std::cout<<"Rejected the new state, probability("<<p<<").\n";
				}
			}	

			temp *= cf; // cooling down

			//std::cout<<"==========\n";
		}
		sa_pack_timer.stop();
		std::cout<<"In the current state: \n";
		std::cout<<"Area coverage = "<<cur_coverage<<',';
		std::cout<<"min = "<<minf<<", max = "<<maxf<<std::endl;
		std::cout<<"Time elapsed: "<<sa_pack_timer.time()/60.0<<" minutes.\n";
		std::cout<<"DONE!\n";
	}
	void Packer::shrink(double factor)
	{
		double min_factor = std::numeric_limits<double>::max();
		for (unsigned int i = 0; i < nb_polytopes(); i++)
			min_factor = std::min(min_factor, polygon_set[i]->get_scale_factor());
		for_each(polygon_set.begin(), polygon_set.end(), std::bind2nd(std::mem_fun(&Packing_polygon::scale), min_factor));
		update_dt();
	}

	void Packer::get_bbox(Geex::real& x_min, Geex::real& y_min, Geex::real& z_min, Geex::real& x_max, Geex::real& y_max, Geex::real& z_max)
	{
		x_min = cbox->x_min(); x_max = cbox->x_max();
		y_min = cbox->y_min(); y_max = cbox->y_max();
		z_min = cbox->z_min(); z_max = cbox->z_max();
	}

}
